/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer/mapping/internal/collated_trajectory_builder.h"

#include "cartographer/common/time.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        namespace
        {
            constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;

        } // namespace

        CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
            const proto::TrajectoryBuilderOptions &trajectory_options,
            sensor::CollatorInterface *const sensor_collator, const int trajectory_id,
            const std::set<SensorId> &expected_sensor_ids,
            std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
            : sensor_collator_(sensor_collator),
              collate_landmarks_(trajectory_options.collate_landmarks()),
              collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
              trajectory_id_(trajectory_id),
              wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
              last_logging_time_(std::chrono::steady_clock::now())
        {
            absl::flat_hash_set<std::string> expected_sensor_id_strings;
            for (const auto &sensor_id : expected_sensor_ids)
            {
                if (sensor_id.type == SensorId::SensorType::LANDMARK && !collate_landmarks_)
                {
                    continue;
                }
                if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE && !collate_fixed_frame_)
                {
                    continue;
                }
                expected_sensor_id_strings.insert(sensor_id.id);
            }
            sensor_collator_->AddTrajectory(
                trajectory_id, expected_sensor_id_strings,
                [this](const std::string &sensor_id, std::unique_ptr<sensor::Data> data) {
                    HandleCollatedSensorData(sensor_id, std::move(data));
                });
        }

        // 传感器的数据都会抽象成sensor::Data加到sensor_collator_中，sensor_collator_中通过队列将这个轨迹缓存的所有传感器数据
        // 按照时间进行排序，然后把最前面的数据分发出来，比如说当前的数据是IMU，而队列里面还有更早的scan数据没有分发出去，那么
        // sensor_collator_里面会先把当前的IMU数据存起来，把之前收到的scan数据分发出去然后传递给前端。就是说收到的各个传感器数
        // 据其时间顺序可能是乱的，但是sensor_collator会重排序，确保前端建图和后端优化模块是按照时间先后顺序来处理数据。
        void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data)
        {
            sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
        }

        /// @brief 接收OrderedMultiQueue分发的数据;这个数据会传给GlobalTrajectoryBuilder;然后GlobalTrajectoryBuilder
		/// 将数据传给LocalTrajectoryBuilder和PosePraph
        /// @param sensor_id 
        /// @param data 
        void CollatedTrajectoryBuilder::HandleCollatedSensorData(const std::string &sensor_id, std::unique_ptr<sensor::Data> data)
        {
            auto it = rate_timers_.find(sensor_id);
            if (it == rate_timers_.end())
            {
                it = rate_timers_.emplace(std::piecewise_construct, std::forward_as_tuple(sensor_id), std::forward_as_tuple(common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))).first;
            }
            it->second.Pulse(data->GetTime());
            if (std::chrono::steady_clock::now() - last_logging_time_ > common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))
            {
                for (const auto &pair : rate_timers_)
                {
                    LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
                }
                last_logging_time_ = std::chrono::steady_clock::now();
            }
            data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
        }
    } // namespace mapping
} // namespace cartographer
